	/*
	 ============================================================================
	 Name        : fgfscontrol.c
	 Author      : Sergio Trimboli, Marcello Piliego
	 Version     :
	 Copyright   :  GNU GPL 2
	 Description : FlightGear Roll-Pitch PID controller by telnet for a Cessna C172P. Si utilizzano due controllori disaccoppiati (unpaired).
	   					 Parametri PID Roll: 				 							Parametri PID Pitch:
	   					 Kp=0.02, Ki=0.001, Kd=0.001.   					 Kp=0.02, Ki=0.001, Kd=0.001.
	   					 Si sfrutta l'interfaccia telnet disponibile in Flightgear con i seguenti parametri:
	   					 $ fgfs --telnet=socket,bi,120,192.168.0.2,tcp
	   					 (in particolare la frequenza di aggiornamento delle variabili (lato telnet remoto) è di 30 Hz;	 
	   					 Infine si calcolano 
	   					 	- n. "MAX_TIMES" tempi parziali e il tempo totale di esecuzione del loop di controllo;
	   					 	- Tempi parziali e tempo totale dell'applicazione telnet lato client nel loop di controllo (scritture e letture sul socket);
	 ============================================================================
	 */
	
	#include <stdio.h>
	#include <errno.h>
	#include <stdlib.h>
	#include <unistd.h>
	#include <sys/time.h>
	#include <sys/types.h>
	#include <sys/socket.h>
	#include <netdb.h>
	#include <netinet/in.h>
	#include <stdarg.h>
	#include <string.h>
	#include <math.h>
	#include <time.h>
	
	#define DFLTHOST		"192.168.0.1" //server Flightgear
	#define DFLTPORT		5501
	#define MAXMSG       256
	#define fgfsclose	    close
	
	#define MAX_TIMES 200 //dimensione vettore tempo (#iterazioni)

	#define RifAlt 100		//Riferimento Altitudine
	#define RifH 225			//Riferimento Angolo di Head
	#define Max_Roll 25		//Massimo Angolo di Roll
	#define Max_Pitch 20	//Massimo Angolo di Pitch
	#define Max_DAlt 100	//Massima Variazione di Altitudine in 60 secondi

	#define Pi 3.141593		//Pi greco

	#define FREQ 1660 //frequenza del processore in MHz
	void init_sockaddr(struct sockaddr_in *name, const char *hostname,unsigned port);
	int fgfswrite(int sock, char *msg, ...);
	const char *fgfsread(int sock, int wait);
	void fgfsflush(int sock);
	
	int fgfswrite(int sock, char *msg, ...) {
		va_list va; //va_list è il tipo di altri argomenti da passare alla funzione fgfswrite
		ssize_t len; //signed size_t
		char buf[MAXMSG]; //buffer da MAXMSG=256
		
		//inizio gestione degli argomenti variabili 
		va_start(va, msg);	//inizializzo la variabile va in modo che punti all'ultimo dei parametri fissi della funzione (msg)
		vsnprintf(buf, MAXMSG - 2, msg, va); //operazioni di lettura sugli altri argomenti passati alla funzione fgfswrite
		va_end(va); //chiudo le operazioni di lettura
		//fine gestione argomenti variabili
		
		/*DEBUG: Stampa del messaggio inviato al socket
		//printf("SEND: \t<%s>\n", buf); //stampo il messaggio che invio al socket
		 */
	
		strcat(buf, "\015\012"); //concatenazione del terminatore di comando con buf
	
		len = write(sock, buf, strlen(buf)); //scrittura del buf sul socket, poi il remoto farà una read()
	
			if (len < 0) { //gestione errori di write()
			perror("fgfswrite");
			exit(EXIT_FAILURE);
			}
		return len; //argomento di ritorno (è un int)
	}
	
	const char *fgfsread(int sock, int timeout) {
		static char buf[MAXMSG]; //inizializzazione buffer (conterrà il messaggio da inviare al server
		char *p;
		fd_set ready; //sostanzialmente è un bit array, è un tipo che serve per la select()
		struct timeval tv;
		ssize_t len;
	
		FD_ZERO(&ready); //This macro initializes the file descriptor set "ready" to be the empty set.
		FD_SET(sock, &ready);//This macro adds "sock" to the file descriptor set "ready". (setta i bits)
		tv.tv_sec = timeout; //per il timeout
		tv.tv_usec = 0;
		
		if (!select(32, &ready, 0, 0, &tv))//The select function blocks the calling process until there is activity 
			return NULL;							//on any of the specified sets of file descriptors, or until the timeout 
														//period has expired. (BLOCCANTE, aspetto che il socket sia pronto per la lettura)
		
		len = read(sock, buf, MAXMSG - 1); //read() del socket
		
		if (len < 0) { //gestione errori di read del socket
			perror("fgfsread"); 
			exit(EXIT_FAILURE);
		}
		
		if (len == 0)
			return NULL; //cioè la stringa letta è vuota
	
		for (p = &buf[len - 1]; p >= buf; p--)
			if (*p != '\015' && *p != '\012')
				break;
		*++p = '\0';
		return strlen(buf) ? buf : NULL; //geniale contrazione di if(strlen(buf)) then (return buf) else (return NULL) :-P
		}
	
	void fgfsflush(int sock) {
		const char *p;
		while ((p = fgfsread(sock, 0)) != NULL) {
			printf("IGNORE: \t<%s>\n", p); //pulitura del buffer
		}
	}
	
	int fgfsconnect(const char *hostname, const int port) {
		struct sockaddr_in serv_addr; //indirizzo server
		struct hostent *hostinfo;
		int sock;
	
		sock = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
		if (sock < 0) {
			perror("fgfsconnect/socket");
			return -1;
		}
	
		hostinfo = gethostbyname(hostname);
		if (hostinfo == NULL) {
			fprintf(stderr, "fgfsconnect: unknown host: \"%s\"\n", hostname);
			close(sock);
			return -2;
		}
	
		serv_addr.sin_family = AF_INET;
		serv_addr.sin_port = htons(port);
		serv_addr.sin_addr = *(struct in_addr *)hostinfo->h_addr;
	
		if (connect(sock, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) < 0) { //connessione socket
			perror("fgfsconnect/connect");
			close(sock);
			return -3;
		}
		return sock; //ritorna il valore del socket
	}
	
	__inline__ unsigned long long int rdtsc()
	{
		unsigned long long int x;
		__asm__ volatile (".byte 0x0f, 0x31" : "=A" (x));
		return x;
	}
	
	int main(int argc, char **argv) {
		//inizio variabili temporizzazione
		unsigned long long int tempo[MAX_TIMES+1]; //tempi i-esimi aggiornato ad ogni iterazione del loop di controllo 
		unsigned long long int tmpt=0; //temporaneo per tempi parziali
		unsigned long long int tmpttelnet=0; //temporaneo per tempi telnet
		unsigned long long int ttelnet[MAX_TIMES+1]; //tempi telnet
		unsigned long long int parz=0; //temporaneo per tempi parziali di esecuzione del loop di controllo
		unsigned long long int tot=0; //tempo di esecuzione totale del loop di controllo
		unsigned long long int temporaneo=0; //temporaneo calcolo tempo telnet
		unsigned long long int temporaneo1=0;//temporaneo calcolo tempo telnet
		//fine variabili temporizzazione
	
		int sock;
		int i=0,l=0;
		unsigned port;
		const char *hostname, *roll, *elev, *head, *pitch;
			
		double errorAlt=0, errorR=0, errorY=0, errorP=0;
		double PropP=0, PropR=0, PropY=0;
		double IntegP=0, IntegR=0, IntegY=0;
		double DerivP=0, DerivR=0, DerivY=0;
		double uP=0, uR=0, uY=0;
		double Alt=0, thetaR=0, thetaY=0, thetaP=0;
		double UppLim=1, LowLim=-1;
		double errorPrecP=0, errorPrecR=0, errorPrecY=0, etR=0, etH=0;
		double RifP=0;

		hostname = argc > 1 ? argv[1] : DFLTHOST;
		port = argc > 2 ? atoi(argv[2]) : DFLTPORT;
	
		sock = fgfsconnect(hostname, port);
		if (sock < 0)
			return EXIT_FAILURE;
		
		// inizializzazione vettore tempi 
		for (i=0; i<=MAX_TIMES; i++) {
			tempo[i] = 0;
			ttelnet[i] = 0;
		};

		for (l=0; l<=MAX_TIMES; l++) {
			tmpt = rdtsc();
			
			tmpttelnet = rdtsc();
			
			fgfswrite(sock, "get /orientation/roll-deg");			 // Acquisizione dell'angolo
			roll = fgfsread(sock, 3);										 // di roll
			thetaR = strtod(roll,NULL); 

			fgfswrite(sock, "get /position/altitude-ft");				// Acquisizione dell'altitudine
			elev = fgfsread(sock, 3);	// di pitch
			Alt = strtod(elev,NULL);

			fgfswrite(sock, "get /orientation/heading-magnetic-deg");			// Acquisizione dell'angolo
			head = fgfsread(sock, 3);															// di Heading
			thetaY = strtod(head,NULL);	

			fgfswrite(sock, "get /orientation/pitch-deg");			// Acquisizione dell'angolo
			pitch = fgfsread(sock, 3);										// di Pitch
			thetaP = strtod(pitch,NULL);								

			ttelnet[l]=rdtsc() - tmpttelnet;
				
			errorY = RifH - thetaY;
			if(errorY >= 180) {errorY = errorY - 360;};
			if(errorY < -180) {errorY = 360 + errorY;};
			if(errorY > Max_Roll) {errorR = Max_Roll - thetaR;};
			if(errorY < -Max_Roll) {errorR = -Max_Roll - thetaR;};
			if(errorY < Max_Roll && errorY > -Max_Roll) {errorR = errorY - thetaR;};
			PropR = 0.02*errorR;
			IntegR = IntegR + (errorR*0.001 + etR);
			if(IntegR>UppLim){IntegR=UppLim;};							//
			if(IntegR<LowLim){IntegR=LowLim;};							//
			DerivR = 0.001*(errorR - errorPrecR);
			if(DerivR>UppLim){DerivR=UppLim;};
			if(DerivR<LowLim){DerivR=LowLim;};
			errorPrecR = errorR;
	
			uR = PropR + IntegR + DerivR;
			printf("uR %f\n",uR);
			if(uR > 1) {etR = 1 - uR;};
			if(uR < -1) {etR = -1 - uR;};
			if(uR < 1 && uR > -1) {etR = 0;};
			//printf("u Roll %f \n", uR);
			
			errorAlt = RifAlt - Alt;
			if(errorAlt >= 0){
				//if(errorAlt > Max_DAlt) {errorAlt = Max_DAlt;};
				RifP = atan(errorAlt/60)*180/Pi;
			}
			if(errorAlt < 0){
				//if(errorAlt < -Max_DAlt) {errorAlt = -Max_DAlt;};
				RifP = atan(errorAlt/60)*180/Pi;
			}
			printf("RifP = %f\n",RifP);
			//if(RifP > Max_Pitch) {RifP = Max_Pitch;};
			//if(RifP < -Max_Pitch) {RifP = -Max_Pitch;};
			RifP = RifP/90*Max_Pitch;
			errorP = (RifP - thetaP);
			PropP=errorP*0.05;											//
			IntegP=IntegP+(errorP*0.001 + etH);									//
			if(IntegP>UppLim){IntegP=UppLim;};							//
			if(IntegP<LowLim){IntegP=LowLim;};							//
			DerivP=(errorP - errorPrecP)*0.001;							//
			if(DerivP>UppLim){DerivP=UppLim;};							//
			if(DerivP<LowLim){DerivP=LowLim;};		
			errorPrecP = errorP;					//
			//uP = (PropP+IntegP+DerivP);
		
			uP = -(PropP+IntegP+DerivP)*cos(thetaR*Pi/180);	
			printf("uP %f\n",uP);
			uY = -(PropP+IntegP+DerivP)*sin(thetaR*Pi/180);	
			
			if(uP > 1) {etH = 1 - uP;};
			if(uP < -1) {etH = -1 - uP;};
			if(uP < 1 && uP > -1) {etH = 0;};
	
	
	
			//printf("u Yaw %f \n",uY);
			//printf("u Pitch%f \n",uP);	
			
			tmpttelnet = rdtsc();
			fgfswrite(sock, "data"); 									//con o senza è lo stesso
			fgfswrite(sock, "set /controls/flight/aileron %f ", uR);   	//scrivo il comando su fgfs, forse è meglio solo con 2 decimali

			//fgfswrite(sock, "data"); //con o senza è lo stesso
			fgfswrite(sock, "set /controls/flight/elevator %f ", uP);   //scrivo il comando su fgfs, forse è meglio solo con 2 decimali

			//	fgfswrite(sock, "data"); //con o senza è lo stesso
			fgfswrite(sock, "set /controls/flight/rudder %f ", uY);   //scrivo il comando su fgfs, forse è meglio solo con 2 decimali
			
			ttelnet[l] = ttelnet[l] + (rdtsc() - tmpttelnet);
			
			tempo[l]=rdtsc()-tmpt; //lettura counter e piazzamento in i (i-esima iterazione), sul loop di controllo
			//printf("rdtsc rdtsc - tmpt %llu \n",tempo[l]);
			};

		//calcolo e stampa dei tempi parziali e del tempo totale di esecuzione del loop di controllo
		parz=0;	
		tot=0;
		for (i=0; i<=MAX_TIMES; i++) {
			parz = tempo[i]/FREQ;
			printf("tempo i-esimo (%d) in microsecondi: %llu \n",i,parz);
			tot = tempo[i] + tot;
		};
		//calcolo e stampa dei tempi parziali e del tempo totale di telnet
		temporaneo1=0;
		for (i=0; i<=MAX_TIMES; i++) {
			temporaneo=ttelnet[i]/FREQ;
			printf("tempo di telnet i-esimo (%d) in microsecondi: %llu \n ",i,temporaneo);
			temporaneo1 = temporaneo1 + ttelnet[i];
		};
		tot=tot/FREQ;
		printf("tempo totale in microsecondi: %llu \n",tot);
		temporaneo1=temporaneo1/FREQ;
		printf("tempo di telnet totale in microsecondi: %llu \n ",temporaneo1);
		//Fine stampa tempi
		
		//chiusura applicazione telnet
		fgfswrite(sock, "quit");
		fgfsclose(sock); //chiusura socket , fgfsclose è definito come close => fa una close(sock)
		return EXIT_SUCCESS;
	}
